#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <kalman.h>

#define DATA_SIZE 50
#define Q 0.00001
#define R 0.01


// 初始化卡尔曼滤波器
void Kalman_Init(Kalman_Typedef* k, double initial_estimate, double initial_error_cov)
{
    k->estimate = initial_estimate;
    k->error_cov = initial_error_cov;
}

// 预测步骤
void Kalman_predict(Kalman_Typedef* kf, double gyro_rate)
{
    kf->x = gyro_rate;
    kf->estimate += gyro_rate * kf->DT;
    kf->error_cov += Q;
}

// 更新步骤
float Kalman_update(Kalman_Typedef* kf, double accelerometer_reading)
{
    kf->K = kf->error_cov / (kf->error_cov + R);
    kf->estimate += kf->K * (accelerometer_reading - kf->estimate);
    kf->error_cov = (1 - kf->K) * kf->error_cov;
    return kf->estimate;
}